/*******************************************************************************
* This file provides the API for MPXV7002 pressure sensor.
* Also implements the algorithm to calculate the airspeed.
*
* Author: Kong Wai Weng
* Email:  waiweng83@gmail.com
*******************************************************************************/



#include "airspeed.h"



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/

#define AIRSPEED_ADC_CHANNEL	1		// ADC channel for the airspeed sensor.
//#define AIRSPEED_ADC_OFFSET		2335	// The ADC value when airspeed = 0.



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Calculated data.
static float fAirspeed = 0.0f;

// ADC offset when airspeed is 0.
static unsigned long ulAdcOffset = 0;



/*******************************************************************************
* PUBLIC FUNCTION: fGetAirspeed
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ Airspeed (m/s).
*
* DESCRIPTIONS:
* Read the calculated airspeed.
*
*******************************************************************************/
float fGetAirspeed(void)
{
	float fBuffer;
	
	INTERRUPT_PROTECT(fBuffer = fAirspeed);
	return fBuffer;
}	



/*******************************************************************************
* TASK: taskAirspeedLoop
*
* DESCRIPTIONS:
* This task is the main loop to calculate the airspeed base on the data from
* the MPSV7002 differential pressure sensor.
*
*******************************************************************************/
portTASK_FUNCTION(taskAirspeedLoop, pvParameters)
{
	static unsigned int uiLoopCount = 0;
	
	// We need to initialize xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	// Do nothing for the first second to allow the sensor to stabilize.
	while (uiLoopCount++ < 50) {
		// Loop every 20ms (50Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 50);
	}	
	
	// Calculate the average for pressure reference for another 3 seconds.
	uiLoopCount = 0;
	while (uiLoopCount++ < (3 * 50)) {
		
		ulAdcOffset += (unsigned long)uiReadAdc(AIRSPEED_ADC_CHANNEL);
		
		// Loop every 20ms (50Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 50);
	}	
	
	// Calculate the averaged offset.
	ulAdcOffset /= 3 * 50; 
	
	while (1) {
		
		// Make sure the absolute pressure sensor is ready.
		if (xSystemState.bAltitudeReady == 1) {		
		
			// Calculate the pressure (Pa).
			float fDiffPressure = (float)((signed int)uiReadAdc(AIRSPEED_ADC_CHANNEL) - (signed int)ulAdcOffset) / 4095.0f * 3.3f * 3.6f / 2.6f * 1000.0f;
			
			// Make sure it's positive.
			if (fDiffPressure < 0.0f) {
				fDiffPressure = 0.0f;
			}	
			
			// Calculate the air density from absolute pressure.
			float fAirDensity = fGetAbsolutePressure() / (R_SPECIFIC * (fGetTemperature() + 273.15f));
			
			// Calculate the airspeed in m/s.
			// Equation source:
			// http://www.kansasflyer.org/index.asp?nav=Avi&sec=Asi&tab=Theory&pg=4
			float fAirspeedTemp = sqrtf(2.0 * fDiffPressure / fAirDensity);
			INTERRUPT_PROTECT(fAirspeed = fAirspeedTemp);
		}
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiAirspeedLoop = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiAirspeedLoop < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
		
		// Loop every 20ms (50Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 50);
	}	
}	
